/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include "particleMath.h"
#include "tpose.h"


static float normalRandomCache[RANDOM_CACHE_SIZE];
static int isnormalRandomCacheOk = 0;


float getNormalRandomFromCache(){
    int i;
    int rv;
    float scale;
    
    if(!isnormalRandomCacheOk){
        for(i=0;i<RANDOM_CACHE_SIZE;i++){
            normalRandomCache[i] = normalRandomHeavy(); 
            isnormalRandomCacheOk = 1;     
        }
    }
    scale = (float) RANDOM_CACHE_SIZE / (float)RAND_MAX;
    rv = rand(); 
    rv = (int) ((float) rv * scale);
    
    return normalRandomCache[rv];
}


/************************************************************
* PROBABILITY DISTRIBUTION FUNCTIONS 
*************************************************************/


void newRandomSeed(){
	 srand( (unsigned)time( NULL ) );
}


/**
* Returns a (pseudo) random number between 0 and 1.
**/
float uniformRandom(void)
{
  return ( ((float) rand() / (float) RAND_MAX));
}

float normalRandomHeavy()
{
	static float gset;
	static int randomStored = 0;
	float fac,rsq,v1,v2;
	
		
	if(randomStored){
        return gset;
    }
    
	do {
		v1=2.0*uniformRandom()-1.0; //pick two uniform numbers in the square extending
		//from -1 to +1 in each direction, 
		v2=2.0*uniformRandom()-1.0;
		rsq=v1*v1+v2*v2;		//see if they are in the unit circle,
		//mexPrintf("Randomizing!!\n");
	} while (rsq >= 1.0 || rsq == 0.0); //if not try again
	
	fac=sqrt(-2.0*log(rsq)/rsq);
	gset=v1*fac; // can be used also as normal distributed random number!!
	//mexPrintf("DONE Randomizing!!\n");
	return v2*fac;
}

/**********************************************************
Returns a normally distributed deviate with zero mean and unit variance, 
using unifomRandom() as the source of uniform deviates.
***************************************************************/

float normalRandom()
{
	return normalRandomHeavy();
	//return getNormalRandomFromCache();

}

/*********************************************
* Transforms normally distributed random variable (variance 1, mean 0)
* According to the covariance cov
* xyRand: includes the normal random variable, which is returned transformed
* cov[0] = X - var
* cov[1] = XY -covar
* cov[2] = Y - var
***/  
void covRandom(float& xRand, float& yRand, float cov[3]){
	float a,b,c;
	float eig1,eig2;
	float vx1,vy1,vx2,vy2;
	float d;
	float x,y;

	// calculate eigen values

	a = 1.0;
	b = -(cov[0]+cov[2]);
	c = cov[0]*cov[2] - cov[1]*cov[1];

	eig1 = (-b + sqrt(b*b-4*a*c))/(2*a);
	eig2 = (-b - sqrt(b*b-4*a*c))/(2*a);

	// calculate eigen vectors

	vx1 = cov[1];
	vy1 = eig1 - cov[0];
	d = sqrt(vx1*vx1+vy1+vy1);
	vx1 /= d;
	vy1 /= d;

	vx2 = cov[1];
	vy2 = eig2 - cov[0];
	d = sqrt(vx2*vx2+vy2+vy2);
	vx2 /= d;
	vy2 /= d;
	
	// Transform the random variable according to the covariance

	x = xRand * sqrt(eig1);
	y = yRand * sqrt(eig2);
	
	xRand = x * vx1 + y * vx2;
	yRand = x * vy1 + y * vy2;
}
	



/**
* Calculates initial pose particle distribution. 
* Samples from normal distribution, thus the initial position is expected to 
* be somewhat known (e.g. within 3mx3m area with angle uncertainty of 10 deg if 
* approx. 800 particles is used)
*  
* TPose pos initial position estimate
* float Q[3] uncertainty (std) of the initial position
* Q[0] x - uncertainty
* Q[1] y - uncertainty
* Q[2] angle uncertainty
* int size: the number of particles in distribution
* 
* returns normally distributed particle distribution
* OBS! creates TPoseParticleDistribution	
*/
TPoseParticleDistribution getInitialDistribution(TPose initPos,
												 float Q[3], 
												 int size
												 ){
	TPoseParticleDistribution PPD;
	int i;
	
	PPD = newPoseParticleDistribution(size);
	
	for(i=0;i<size;i++){
		PPD.Particles[i].x = initPos.x + normalRandom() * Q[0];
		PPD.Particles[i].y = initPos.y + normalRandom() * Q[1];
		PPD.Particles[i].a = initPos.a + normalRandom() * Q[2];
		PPD.Particles[i].lik = 1.0;
		PPD.Particles[i].p = 1.0 / size;
		PPD.NumOfParticles++;
	}
	return PPD;
}



TPoseParticleDistribution SIRupdate(TPoseParticleDistribution PPD){
	TPoseParticleDistribution tmp;
	float U,Q;
	int i,j,k;
	
	U = uniformRandom() / (float) PPD.NumOfParticles;
	//U = 0.1 / (float) PPD.NumOfParticles;
	j=0;
	Q=0.0;
	i=0;
	k=0;

	tmp = newPoseParticleDistribution(PPD.NumOfParticles);
	
	tmp.Lik = PPD.Lik;
	tmp.outLiers = PPD.outLiers;
	tmp.NumOfParticles = PPD.NumOfParticles;

	while(U < 1.0){
		
		if(Q>U){
			U += 1.0/PPD.NumOfParticles;
			if(k>=PPD.NumOfParticles || i>=tmp.NumOfParticles){
                fprintf(stderr,"ERROR: SIRupdate:: Invalid index k='%d' or i='%d'\n",k,i);
                freePoseParticleDistribution(PPD);
                return tmp;        
            }
			tmp.Particles[i]=PPD.Particles[k];
			tmp.Particles[i].p = 1.0 / PPD.NumOfParticles;
			i++;
		}
		else{
			k=j;
			if(j>=PPD.NumOfParticles){
                fprintf(stderr,"ERROR: SIRupdate:: Invalid index j='%d' \n",j);
                freePoseParticleDistribution(PPD);
                return tmp;        
            }
			Q += PPD.Particles[j].p;
			j++;
		}
	}//While
	
	freePoseParticleDistribution(PPD);

	return tmp;
}


/**
* Makes the prediction step using odometry estimate and covariance matrix
* the x and y noise can be correlated, but the angle noise is independent
* 
* PPD:  Posteriori distribution - returned as priori disribution
* v[3]: odometry estimate (dx,dy,da)
* Q[4]: Uncertainty (Var(X) Var(XY) Var(Y) STD(A)!!)
**/
TPoseParticleDistribution penaPredictCov(TPoseParticleDistribution PPD, 
									  float v[3], 
									  float Q[4])
{
	int i;
	float dx,dy,dl;
	float cov[3];
	float xRand,yRand;
	float a,b,c;
	float eig1=1,eig2=1;
	float vx1,vy1,vx2,vy2;
	float d;
	float x,y;

	// calculate eigen values
	cov[0] = Q[0];
	cov[1] = Q[1];
	cov[2] = Q[2];
	
	a = 1.0;
	b = -(cov[0]+cov[2]);
	c = cov[0]*cov[2] - cov[1]*cov[1];
	
	if( (b*b-4*c)>=0){
		eig1 = (-b + sqrt(b*b-4*a*c))/(2*a);
		eig2 = (-b - sqrt(b*b-4*a*c))/(2*a);
		
		// calculate eigen vectors
		if(eig1 != eig2){
			vx1 = cov[1];
			vy1 = eig1 - cov[0];
			d = sqrt(vx1*vx1+vy1*vy1);
			vx1 /= d;
			vy1 /= d;
			
			vx2 = cov[1];
			vy2 = eig2 - cov[0];
			d = sqrt(vx2*vx2+vy2*vy2);
			vx2 /= d;
			vy2 /= d;
		}
		else{
				vx1 = 1.0;
				vx2 = 0.0;
				vy1 = 0.0;
				vy2 = 1.0;
		}
		
	}
	else{
		fprintf(stderr,"IMAGINARY EIGEN VALUES\n");
		eig1=1;
		eig2=1;
		
		vx1 = 1.0;
		vx2 = 0.0;
		vy1 = 0.0;
		vy2 = 1.0;
	}
	
	
	// Transform the random variable according to the covariance
	
	dl = sqrt(v[0]*v[0]+v[1]*v[1]);
	
	for(i=0;i<PPD.NumOfParticles;i++){
		xRand = normalRandom();
		yRand = normalRandom();
		
		x = xRand * sqrt(eig1);
		y = yRand * sqrt(eig2);
		
		xRand = x * vx1 + y * vx2;
		yRand = x * vy1 + y * vy2;
		//mexPrintf("XR: %.3lf, YR:%.3lf x:%.3lf y%.3lf eig1:%.3lf eig2:%.3lf, %.3lf %.3lf %.3lf %.3lf\n",xRand,yRand,x,y,eig1,eig2,vx1,vy1,vx2,vy2);
		//Sleep(50);
		//covRandom(xRand,yRand,cov);

		dx = dl * cos(PPD.Particles[i].a + v[2]);
		dy = dl * sin(PPD.Particles[i].a + v[2]);
		
		float covX,covY,covA,covL;

		covL = sqrt(xRand*xRand + yRand*yRand);
		covA = atan2(yRand,xRand);
		covX = covL * cos(PPD.Particles[i].a+covA);
		covY = covL * sin(PPD.Particles[i].a+covA);
		
		PPD.Particles[i].x = PPD.Particles[i].x + dx + covX;
		PPD.Particles[i].y = PPD.Particles[i].y + dy + covY;
		PPD.Particles[i].a = PPD.Particles[i].a + v[2] + normalRandom()* Q[3];
	}
return PPD;
}


/**
* Makes the prediction step using odometry estimate and uncertainty, 
* the uncertainties (noise) are expected to be independed 
* 
* PPD:  Posteriori distribution - returned as priori disribution
* v[3]: odometry estimate (dx,dy,da)
* Q[3]: Uncertainty (STD(X) STD(Y) STD(A))
**/

TPoseParticleDistribution penaPredict(TPoseParticleDistribution PPD, 
									  float v[3], 
									  float Q[3])
{
#define DIST_TO_PROB	1.0 / 0.2 //20cm means 1 x Noise
#define ANG_TO_PROB		1.0 / (5.0 * PI / 180.0)
	int i;
	float dx=0.0,dy=0.0,dl=0.0;
	//	float offset =0.0f ; // -PI/2.0f;
	float a=0.0;
	float QQ[3] = { 0.0, 0.0, 0.0};
#ifdef MCL_SAVE_TO_FILE	
	FILE *f;
#endif
	static float x=0,y=0, h=0;	


	dl = sqrt(v[0]*v[0]+v[1]*v[1]);
	a  = atan2(v[1],v[0]);

	x += dl * cos(h+a);
	y += dl * sin(h+a);
	h +=v[2];

#ifdef MCL_SAVE_TO_FILE	
	f = fopen("path.txt","at");
	fprintf(f,"%.3f %.3f %.3f\n",x,y,h);
	fclose(f);
#endif

	if(a<0) a = 2.0f * PI + a;
	
	//fprintf(stderr,"A:%.2f dl:%.2f\n",a,dl);
/*
	if(dl<0.01){
		Q[0] = 0.01f;
		Q[1] = 0.01f;
	}
	else if(dl<0.05){
		Q[0] = 0.02f;
		Q[1] = 0.02f;
	}
	else if(dl<0.1){
		Q[0] = 0.05f;
		Q[1] = 0.05f;
	}
*/	
	QQ[0] = Q[0];
	QQ[1] = Q[1];
	QQ[2] = Q[2];

	if(dl<0.01){
		QQ[0] = Q[0]*0.1f; //0.01f;
		QQ[1] = Q[0]*0.1f;
		//QQ[2] = Q[2]*0.1f;
	}
	else if(dl<0.05){
		QQ[0] = Q[0]*0.5f; //0.02f;
		QQ[1] = Q[0]*0.5f;
	}
	else if(dl<0.1){
		QQ[0] = Q[0]*0.5f;// 0.05f;
		QQ[1] = Q[0]*0.5f;
	}

	for(i=0;i<PPD.NumOfParticles;i++){
		
		dx = dl * cos(PPD.Particles[i].a +a );
		dy = dl * sin(PPD.Particles[i].a +a );

		PPD.Particles[i].x = PPD.Particles[i].x + dx + normalRandom()* QQ[0];
		PPD.Particles[i].y = PPD.Particles[i].y + dy + normalRandom()* QQ[1];
		PPD.Particles[i].a = PPD.Particles[i].a + v[2] + normalRandom()* QQ[2];
	}
return PPD;
}


TPoseParticleDistribution normalizeDistribution(TPoseParticleDistribution PPD){
	int i;
	float summ=0;

	for(i=0;i<PPD.NumOfParticles;i++){
		PPD.Particles[i].p *= PPD.Particles[i].lik; 
		summ+=PPD.Particles[i].p;
	}
	for(i=0;i<PPD.NumOfParticles;i++){
		PPD.Particles[i].p = PPD.Particles[i].p/summ;
	}
	return PPD;
}



TPoseParticleDistribution normalizeASIRDistribution(
													TPoseParticleDistribution PPD,
													TPoseParticleDistribution PPD2)
{
	int i;
	float summ=0;

	for(i=0;i<PPD.NumOfParticles;i++){
		PPD.Particles[i].p *= PPD.Particles[i].lik / PPD2.Particles[i].lik; 
		summ+=PPD.Particles[i].p;
	}
	for(i=0;i<PPD.NumOfParticles;i++){
		PPD.Particles[i].p = PPD.Particles[i].p/summ;
	}
	return PPD;
}




/**********************************************************************
* getScanLines
* Returns line parameters (a,b,c) ax+by+c=0 for one scan taken from given position
*
* @param *scanLines		Output: The lineparameter representing the scan. 
*						Size should be 3*@number_of_measurements
* @param P				The position to which the lines are calculated
* @param number_of_measurements The number of lines in scan
* @param laserOffset	The offset between the robot coordinatesystem and laser coordinate system	
*
***************************************************/ 
void getScanLines(float *scanLines,
				  TPoseParticle P, 
				  int number_of_measurements,
				  float laserOffset){
	int i;
	float alpha;
	float phii;

	// Rotation to the laser coordinates
	phii=P.a+laserOffset;

	//mexPrintf("PHII: %lf\n",phii);
	if(phii>2.0*PI) phii-= (float)(2.0*PI);
	if(phii<-2.0*PI) phii+=(float)(2.0*PI);
	
	for(i=0;i<number_of_measurements;i++){
		alpha=phii+(180.0 / number_of_measurements) * (PI/180.0)*i ;

		if( (fabs( fabs( alpha ) - PI/2.0 )) <0.01 || (fabs( fabs( alpha ) - 3.0*PI/2.0 )) <0.01  ){
			scanLines[3*i]=1;
			scanLines[3*i+1]=0;
			scanLines[3*i+2]=-P.x;
		}
		else{
			scanLines[3*i]=-tan(alpha);
			scanLines[3*i+1]=1;
			scanLines[3*i+2]=tan(alpha)*P.x-P.y;
		}
	}
}


/************************************************
* Calculates virtual scan from line map
* returns *Scan (x, y) in robot coordinate system
* Function is required for likehood calculations
**************************************************/
int findVirtualScan(float *Scan, 
					 float *scanLines, 
					 TLineVector map,
					 TPoseParticle Pos,
					 int number_of_measurements){
	int i;
	int j;
	float a1,b1,c1,a2,b2,c2; //Line parameters
	float x0=0.0,y0=0.0,x=0.0,y=0.0,px=0.0,py=0.0,xr=0.0,yr=0.0,Ret_xr=100,Ret_yr=100;
	float mx1,my1,mx2,my2,L,AC,AB;
	float DIST,distTmp;
	float S,C;
	float step;
	int isIt;
	int outLiers=0;


	S=sin(Pos.a);//NOTE THIS YARI-PI/2);
	C=cos(Pos.a);//NOTE THIS YARI-PI/2);

	step = PI / (float) number_of_measurements;

	for(i=0;i<number_of_measurements;i++){
		a1=scanLines[3*i];
		b1=scanLines[3*i+1];
		c1=scanLines[3*i+2];
		DIST=LASER_MAX_DIST2; //Note map distances are in CM's !!
		x=1000;
		y=1000;
		Ret_xr=1000;
		Ret_yr=1000;
		for(j=0;j<map.NumOfLines;j++){
			isIt=1;
			a2=map.Lines[j].p.a; // Lines[j*3];
			b2=map.Lines[j].p.b; // Lines[j*3+1];
			c2=map.Lines[j].p.c; // Lines[j*3+2];

			if( fabs(a1*b2-a2*b1)<0.000001 ){
				//mexPrintf("Parallel\n");
				isIt=0; //Lines are parallel
			}
			else
			{
				//Calculation of intersection
				x0 = (-c1*b2 + c2*b1) / (a1*b2-a2*b1);
				y0 = (-c2*a1 + a2*c1) / (a1*b2-a2*b1);
				
				px=(x0-Pos.x);
				py=(y0-Pos.y);
				
				xr=C * px + S * py;
				yr=-S * px + C * py; //xr,yr in robot coordinate system
				
				if(xr>=0) isIt=1; 
				else{
					//mexPrintf("COORDINATES\n");
					isIt=0; //The intersection was not in right direction
				}
				
				if(i==0 && yr>0) isIt=0;
				
			}// ELSE
			
			if(isIt){
				mx2=map.Lines[j].x1; //map[j*N];
				my2=map.Lines[j].y1; //map[j*N+1];
				mx1=map.Lines[j].x2; //map[j*N+2];
				my1=map.Lines[j].y2; //map[j*N+3];

				L=( (mx2-mx1)*(mx2-mx1) + (my2-my1)*(my2-my1) );
				AC=( (x0-mx2)*(x0-mx2) + (y0-my2)*(y0-my2) );
				AB=( (x0-mx1)*(x0-mx1) + (y0-my1)*(y0-my1) );
				if(AC<=L && AB<=L ) isIt=1;
				else{
					//mexPrintf("NOT INSIDE\n");
					isIt=0; // not inside the map line segment
				}
			}
			distTmp = ( px*px + py*py );

			if( distTmp <= DIST && isIt==1){
				x=x0;
				y=y0;
				DIST=distTmp; //closest hit so far
				Ret_xr = xr; // In "robot" coordinate system
				Ret_yr = yr; // In "robot" coordinate system
			}
			//else mexPrintf("DIST TAI JOKU AIEMPI\n");

		}//FOR J
		//Scan[2*i]=x;  // FOR ABSOLUTE COORDINATES
		//Scan[2*i+1]=y;
		
		//Scan[2*i]=DIST * cos(i * step + Pos.a-PI/2.0);   // FOR RELATIVE COORDINATES
		//Scan[2*i+1]=DIST * sin(i * step + Pos.a-PI/2.0);
		Scan[2*i]=Ret_xr;
		Scan[2*i+1]=Ret_yr;
		
		if(Ret_xr==1000)outLiers++;

		//mexPrintf("Xr:%lf Yr:%lf\n",Ret_xr,Ret_yr);
	}//for I

return outLiers;
}


/************************************************
* Calculates virtual scan from line map
* 
* @param *r				Output: The range measurements. If not used set as NULL. 
*						The size should be same as @number_of_measurements
* @param *xyscan_rel	Output: The relative x,y coordinates.If not used set as NULL.
*						The size should be same as 2 * @number_of_measurements
* @param *xyscan_abs	Output: The absolute x,y coordinates.If not used set as NULL.
*						The size should be same as 2 * @number_of_measurements
* @param Pos			The position where the robot is assumed to be
* @param map			The linemap
*
* @param number_of_measurements The number of laser beams to be calculated
* @param laserOffset	The angle difference between the robot coordinate system and laser coordinate system.
*						e.g. If your robot is travelling along X-axis in zero heading, the laser is probably 
*						pointing forward, thus the first measurement (0 angle from laser point of view) is from 
						-PI/2 from the robot perspective. In this case the offset is -PI/2
*
*/
int VirtualScan(float *r,
					  float *xyscan_rel,
					  float *xyscan_abs,
					  TPoseParticle Pos,
					  TLineVector map, 
					  int number_of_measurements,
					  float laserOffset){

	int i;
	int j;
	float a1,b1,c1,a2,b2,c2; //Line parameters
	float x0=0.0,y0=0.0,x=0.0,y=0.0,px=0.0,py=0.0,xr=0.0,yr=0.0,Ret_xr=100,Ret_yr=100;
	float mx1,my1,mx2,my2,L,AC,AB;
	float DIST,distTmp;
	float S,C;
	float step;
	int isIt;
	int outLiers=0;
	float *scanLines;

	scanLines = (float *) malloc(3 * sizeof(float) * number_of_measurements);

	getScanLines(scanLines, Pos, number_of_measurements, laserOffset); 	

	S=sin(Pos.a);//NOTE THIS YARI-PI/2);
	C=cos(Pos.a);//NOTE THIS YARI-PI/2);

	step = PI / (float) number_of_measurements;

	for(i=0;i<number_of_measurements;i++){
		a1=scanLines[3*i];
		b1=scanLines[3*i+1];
		c1=scanLines[3*i+2];
		DIST=LASER_MAX_DIST2; //Note map distances are in CM's !!
		x=1000;
		y=1000;
		Ret_xr=1000;
		Ret_yr=1000;
		for(j=0;j<map.NumOfLines;j++){
			isIt=1;
			a2=map.Lines[j].p.a; // Lines[j*3];
			b2=map.Lines[j].p.b; // Lines[j*3+1];
			c2=map.Lines[j].p.c; // Lines[j*3+2];

			if( fabs(a1*b2-a2*b1)<0.000001 ){
				//mexPrintf("Parallel\n");
				isIt=0; //Lines are parallel
			}
			else
			{
				//Calculation of intersection
				x0 = (-c1*b2 + c2*b1) / (a1*b2-a2*b1);
				y0 = (-c2*a1 + a2*c1) / (a1*b2-a2*b1);
				
				px=(x0-Pos.x);
				py=(y0-Pos.y);
				
				xr=C * px + S * py;
				yr=-S * px + C * py; //xr,yr in robot coordinate system
				
				if(xr>=0) isIt=1; 
				else{
					//mexPrintf("COORDINATES\n");
					isIt=0; //The intersection was not in right direction
				}
			}// ELSE
			
			if(isIt){
				mx2=map.Lines[j].x1; //map[j*N];
				my2=map.Lines[j].y1; //map[j*N+1];
				mx1=map.Lines[j].x2; //map[j*N+2];
				my1=map.Lines[j].y2; //map[j*N+3];

				L=( (mx2-mx1)*(mx2-mx1) + (my2-my1)*(my2-my1) );
				AC=( (x0-mx2)*(x0-mx2) + (y0-my2)*(y0-my2) );
				AB=( (x0-mx1)*(x0-mx1) + (y0-my1)*(y0-my1) );
				if(AC<=L && AB<=L ) isIt=1;
				else{
					//mexPrintf("NOT INSIDE\n");
					isIt=0; // not inside the map line segment
				}
			}
			distTmp = ( px*px + py*py );

			if( distTmp <= DIST && isIt==1){
				x=x0;
				y=y0;
				DIST=distTmp; //closest hit so far
				Ret_xr = xr; // In "robot" coordinate system
				Ret_yr = yr; // In "robot" coordinate system
			}
			//else mexPrintf("DIST TAI JOKU AIEMPI\n");

		}//FOR J
		//Scan[2*i]=x;  // FOR ABSOLUTE COORDINATES
		//Scan[2*i+1]=y;
		
		r[i] = DIST;

		//xyscan_rel[2*i]=DIST * cos(i * step + Pos.a-PI/2.0);   // FOR RELATIVE COORDINATES
		//xyscan_rel[2*i+1]=DIST * sin(i * step + Pos.a-PI/2.0);
		
		//
		xyscan_rel[2*i]=Ret_xr;
		xyscan_rel[2*i+1]=Ret_yr;
		
		xyscan_abs[2*i] = x;
		xyscan_abs[2*i+1] = y;
		
		if(Ret_xr==1000)outLiers++;

		//mexPrintf("Xr:%lf Yr:%lf\n",Ret_xr,Ret_yr);
	}//for I
	
	free(scanLines);

return outLiers;

}

void testVirtualScan(TPoseParticle Pos,
					  TLineVector map, 
					  int number_of_measurements,
					  float laserOffset){
	float *scan_rel, *scan_abs, *r;
	FILE *f1,*f2,*f3;
	int i;

	r = (float *) malloc(number_of_measurements * sizeof(float));
	scan_rel = (float *) malloc(number_of_measurements * sizeof(float) * 2);
	scan_abs = (float *) malloc(number_of_measurements * sizeof(float)*2);

	VirtualScan(r,scan_rel,scan_abs,Pos,map,number_of_measurements,laserOffset);
	
	f1 = fopen("r.txt","wt");
	f2 = fopen("rel.txt","wt");
	f3 = fopen("abs.txt","wt");

	for(i=0;i<number_of_measurements;i++){
		fprintf(f1,"%.2f\n",r[i]);
		fprintf(f2,"%.2f ",scan_rel[2*i]);
		fprintf(f2,"%.2f\n",scan_rel[2*i+1]);
		fprintf(f3,"%.2f ",scan_abs[2*i]);
		fprintf(f3,"%.2f\n",scan_abs[2*i+1]);
	}

	fclose(f1);
	fclose(f2);
	fclose(f3);
	free(r);
	free(scan_abs);
	free(scan_rel);

}

/************************************************
* Calculates virtual scan from line map
* This is improved algorithm from the one presented above
* 
* @param *r				Output: The range measurements. If not used set as NULL. 
*						The size should be same as @number_of_measurements
* @param *xyscan_rel	Output: The relative x,y coordinates.If not used set as NULL.
*						The size should be same as 2 * @number_of_measurements
* @param *xyscan_abs	Output: The absolute x,y coordinates.If not used set as NULL.
*						The size should be same as 2 * @number_of_measurements
* @param *scanLines		The calculated scanlines 
* @param Pos			The position where the robot is assumed to be
* @param map			The linemap
*
* @param number_of_measurements The number of laser beams to be calculated
* @param laserOffset	The angle difference between the robot coordinate system and laser coordinate system.
*						e.g. If your robot is travelling along X-axis in zero heading, the laser is probably 
*						pointing forward, thus the first measurement (0 angle from laser point of view) is from 
						-PI/2 from the robot perspective. In this case the offset is -PI/2
*
*/
int findVirtualScan2(float *r,
					  float *xyscan_rel,
					  float *xyscan_abs,
					  float *scanLines,
					  TPoseParticle Pos,
					  TLineVector map, 
					  int number_of_measurements,
					  float laserOffset){
    
    
    int i,j,start,end;
	int outliers=0;
    float a1,b1,c1,a2,b2,c2; //Line parameters
    float x0,y0,px=100,py=100;
    float mx1,my1,mx2,my2;
	float rTmp[512]; //temporarily store the results here (note max number of meas is 512)
    float phii,alpha;
    float range,fac,ifac,num;

	if(number_of_measurements > 511 || number_of_measurements<0){
		fprintf(stderr,"findVirtualScan2:: Invalid number of measurements\n");
		r = NULL;
		xyscan_rel = NULL;
		xyscan_abs = NULL;
		return -1;
	}
	
	
    fac=number_of_measurements/PI;
    ifac=1/fac;
	// Rotate to the laser coordinate system
    phii=Pos.a + laserOffset;
    
    while(phii < -PI) {
	phii += (float)(2.0f*PI);
    }
    while(phii > PI) {
	phii -= (float)(2.0f*PI);
    }

    for(i=0;i<number_of_measurements;i++)
    {
		rTmp[i]= 81.5f*81.5f;
        alpha=(float)phii+(float)(((float)i)*ifac) ;
        scanLines[3*i]=(float)-sin(alpha);
        scanLines[3*i+1]=(float)cos(alpha);
        scanLines[3*i+2]=(float)(-scanLines[3*i]*Pos.x-scanLines[3*i+1]*Pos.y);
        
        
    }
    
    for(i=0;i<map.NumOfLines;i++){
        
        mx2=map.Lines[i].x1; //map[i*N];
        my2=map.Lines[i].y1; //map[i*N+1];
        mx1=map.Lines[i].x2; //map[i*N+2];
        my1=map.Lines[i].y2; //map[i*N+3];
        
      //  b2=mx1-mx2;
      //  a2=my2-my1;
      //  c2=-mx1*a2-my1*b2;
        a2=map.Lines[i].p.a; // Lines[i*3];
        b2=map.Lines[i].p.b; // Lines[i*3+1];
        c2=map.Lines[i].p.c; // Lines[i*3+2];
 //       d=a2*Pos.x+b2*Pos.y+c2;
        px=(mx1-Pos.x);
        py=(my1-Pos.y);
        
        //calculate angles at which the endpoints are seen
        start=(int)floor((atan2(py,px)-phii)*fac);
        
        px=(mx2-Pos.x);
        py=(my2-Pos.y);
        end=(int)floor((atan2(py,px)-phii)*fac);
        
        if(end<0)
            end=(number_of_measurements * 2 - 1)+end;
        if(start<0)
            start=(number_of_measurements * 2 - 1)+start;
        if(end<start)
        {
            j=start;
            start=end;
            end=j;
        }
        if((end-start)>number_of_measurements-1)
        {
            end=start;
            start=0;
        }
        if(end>number_of_measurements)
            end=number_of_measurements;  
        
        for(j=start;j<end;j++){
            a1=scanLines[3*j];
            b1=scanLines[3*j+1];
            c1=scanLines[3*j+2];
            num=1/(a1*b2-a2*b1);
            //Calculation of intersection
            x0 = (c2*b1-c1*b2)*num ;
            y0 = (a2*c1-c2*a1)*num;
            
            px=(x0-Pos.x); // Relative coordinates
            py=(y0-Pos.y); // Relative coordinates
            range=(px*px + py*py);
            if(rTmp[j]>range)
            {
                rTmp[j]=range;
				// Store the relative x,y-coordinates also
				if(xyscan_rel != NULL){
					xyscan_rel[2*j]=	px;
					xyscan_rel[2*j+1] = py;
				}
				// Store the absolute x,y-coordinates also
				if(xyscan_abs != NULL){
					xyscan_abs[2*j]=	x0;
					xyscan_abs[2*j+1] = y0;
				}

            }
        }
    }
    
	if(r != NULL){
		for(i=0;i<number_of_measurements;i++){
			r[i]=(float)sqrt(rTmp[i]);
			if(r[i]>80.0f) outliers++;
		}
    }

	return outliers;
}


/************************************************
* Find the lines that are visible from pose Pos
* Map is the line map, Pos is the 
**************************************************/
TLineVector findVisibilityLines( 
					 TLineVector map,
					 TPoseParticle Pos
					 ){
	int i;
	int j,k;
	float a1,b1,c1,a2,b2,c2; //Line parameters
	float x0=0.0,y0=0.0,x=0.0,y=0.0,px=0.0,py=0.0,xr=0,yr=0,Ret_xr=100,Ret_yr=100;
	float mx1,my1,mx2,my2,L,AC,AB;
	float DIST,distTmp;
	float S,C;
	int isIt;
	//	int outLiers=0;
	float *scanLines;
	int NUM_OF_SCANS = 180;
	TLineVector LV;
	TLine Line;
	int *lineIndex;
	int tmpIndex;

	scanLines = (float *) malloc(3 * NUM_OF_SCANS * sizeof(float));
	lineIndex = (int *) malloc(NUM_OF_SCANS * sizeof(int));

	LV = newLineVector(NUM_OF_SCANS);
	
	getScanLines(scanLines,Pos, NUMBER_OF_MEASUREMENTS, (float)(-PI/2.0f));
	
	S=sin(Pos.a-PI/2);
	C=cos(Pos.a-PI/2);

	for(i=0;i<NUM_OF_SCANS;i++){
		a1=scanLines[3*i];
		b1=scanLines[3*i+1];
		c1=scanLines[3*i+2];
		DIST=LASER_MAX_DIST2; //Note map distances are in CM's !!
		x=1000;
		y=1000;
		Ret_xr=1000;
		Ret_yr=1000;
		tmpIndex = -1;
		for(j=0;j<map.NumOfLines;j++){
			isIt=1;
			a2=map.Lines[j].p.a; // Lines[j*3];
			b2=map.Lines[j].p.b; // Lines[j*3+1];
			c2=map.Lines[j].p.c; // Lines[j*3+2];

			if( fabs(a1*b2-a2*b1)<0.000001 ){
				//mexPrintf("Parallel\n");
				isIt=0; //Lines are parallel
			}
			else
			{
				//Calculation of intersection
				x0 = (-c1*b2 + c2*b1) / (a1*b2-a2*b1);
				y0 = (-c2*a1 + a2*c1) / (a1*b2-a2*b1);
				
				px=(x0-Pos.x);
				py=(y0-Pos.y);
				
				//xr=C * px + S * py;
				yr=-S * px + C * py; //xr,yr in robot coordinate system
				
				if(yr>=0) isIt=1; 
				else{
					//mexPrintf("COORDINATES\n");
					isIt=0; //The intersection was not in right direction
				}
			}// ELSE
			
			if(isIt){
				mx2=map.Lines[j].x1; //map[j*N];
				my2=map.Lines[j].y1; //map[j*N+1];
				mx1=map.Lines[j].x2; //map[j*N+2];
				my1=map.Lines[j].y2; //map[j*N+3];

				L=( (mx2-mx1)*(mx2-mx1) + (my2-my1)*(my2-my1) );
				AC=( (x0-mx2)*(x0-mx2) + (y0-my2)*(y0-my2) );
				AB=( (x0-mx1)*(x0-mx1) + (y0-my1)*(y0-my1) );
				if(AC<=L && AB<=L ) isIt=1;
				else{
					//mexPrintf("NOT INSIDE\n");
					isIt=0; // not inside the map line segment
				}
			}
			distTmp = ( px*px + py*py );

			if( distTmp <= DIST && isIt==1){
				x=x0;
				y=y0;
				DIST=distTmp; //closest hit so far
				Ret_xr = xr; // In "robot" coordinate system
				Ret_yr = yr; // In "robot" coordinate system
				Line = map.Lines[j];
				tmpIndex = j;

			}
			//else mexPrintf("DIST TAI JOKU AIEMPI\n");

		}//FOR J
		
		if(tmpIndex>=0){ // some line found
			int isNewLine = 1;
			
			for(k=0;k<LV.NumOfLines;k++){ // go through allready marked lines
				if(lineIndex[k]==tmpIndex) isNewLine = 0; //The line was allready in vector
			}

			if(isNewLine){ 
				LV.Lines[LV.NumOfLines] = Line;
				LV.NumOfLines++;
			}
		}

		//mexPrintf("Xr:%lf Yr:%lf\n",Ret_xr,Ret_yr);
	}//for I

	free(lineIndex);
	free(scanLines);

return LV;
}






/*****************************************************
* Calculates the likelihood of particle distribution
* PPD: The particle distribution
* Map: Line map
* Scan: Current measurement in robot coordinate system (x,y)
*************************************************************/

TPoseParticleDistribution calculateLikelihood(
					      TPoseParticleDistribution PPD, 
					      TLineVector Map,
					      float *Scan
					      )
{
  float *scanLines, *virtualScan;
  float r[NUMBER_OF_MEASUREMENTS];
  float r_virt;
  int i,j;

  // JARI: FIXME; Initializing to Zero to satisfy valgrind.
  for(i=0; i < NUMBER_OF_MEASUREMENTS; i++) {
    r[i] = 0.00;
  }
	
  //	float dx,dy;
  float dL2,dl;
  float Lik;
  float Liks;
  int outLiers;
#ifdef MCL_SAVE_TO_FILE	
  FILE *f;
	
  f = fopen("likmeas.txt","wt");
  for(j=0;j<NUMBER_OF_MEASUREMENTS;j++){
    fprintf(f,"%.2f %.2f\n",Scan[2*j],Scan[2*j+1]);
  }
  fclose(f);

  f= fopen("lik.txt","wt");
#endif


  scanLines = (float *) malloc(3 * NUMBER_OF_MEASUREMENTS * sizeof(float));
  virtualScan = (float *) malloc (2 * NUMBER_OF_MEASUREMENTS * sizeof(float));
  PPD.Lik=0;
  for(i=0;i<PPD.NumOfParticles;i++){
		
    getScanLines(scanLines,PPD.Particles[i], NUMBER_OF_MEASUREMENTS, (float)(-PI/2.0f));
		
    /**
       outLiers=findVirtualScan2(r,virtualScan,NULL,scanLines,
       PPD.Particles[i],
       Map,NUMBER_OF_MEASUREMENTS,
       (float)(-PI/2.0f));
    **/
			
			
    outLiers=findVirtualScan(virtualScan,scanLines,
			     Map,PPD.Particles[i],NUMBER_OF_MEASUREMENTS);
			
		
    PPD.outLiers+=(float) outLiers;
		
    Liks = 1;
		
    for(j=0;j<NUMBER_OF_MEASUREMENTS;j++){
      r[j] = sqrt( Scan[2*j]* Scan[2*j] + Scan[2*j+1]*Scan[2*j+1]);
      r_virt = sqrt(virtualScan[2*j]*virtualScan[2*j] + virtualScan[2*j+1]*virtualScan[2*j+1]);

			
      //dx = Scan[2*j] - virtualScan[2*j];
      //dy = Scan[2*j+1] - virtualScan[2*j+1];
      //dL2 = dx*dx + dy*dy;
			
      dl = r[j] - r_virt;		// The difference between expected measurement and real measurement
      /*
	if(dL2 - dl*dl<0.01) fprintf(stderr,"ERROR!! dl2:: %.2f dldl: %.2f r[j]: %.2f r_virt:%.2f\n",dL2,
	dl*dl,
	r[j],r_virt);
      */
      //If the value was ok
      if(r[j]>=0){
				
	if(dl < -1.5f){
	  Lik = 0.4f;
	  Liks = Liks*Lik;
	}
	else{
	  dL2 = dl*dl;
	  Lik = (1.0- FALSE_PROB) * exp(  - (dL2/(VMEAS)) );
	  //fprintf(stderr,"%.2f ",Lik);
	  Lik += FALSE_PROB;
	  Liks = Liks*Lik;
	}
      }
		
      //fprintf(stderr,"%.2f ",Liks);

#ifdef MCL_SAVE_TO_FILE				
      fprintf(f,"%.2f %.2f ",virtualScan[2*j],virtualScan[2*j+1]);
#endif			
      //if( dx< -1.50) {
      //	Lik = exp(  - (dL2/(VMEAS)) ) + 1.5*FALSE_PROB;
      //	Liks = Liks*Lik;
      //}
      //else{
      //	Lik = exp(  - (dL2/(VMEAS)) ) + FALSE_PROB;
      //	Liks = Liks*Lik;
      //}
    }
#ifdef MCL_SAVE_TO_FILE	
    fprintf(f,"\n");
#endif
    //fprintf(stderr,"\n");
    PPD.Lik+=Liks;
    PPD.Particles[i].lik=Liks;
  }
  PPD.outLiers = (PPD.outLiers / ( NUMBER_OF_MEASUREMENTS * PPD.NumOfParticles))*100;
  //debug(DEBUGS,"LIK: %.2f\n",PPD.Lik);

  free(scanLines);
  free(virtualScan);
#ifdef MCL_SAVE_TO_FILE	
  fclose(f);
#endif
  return PPD;
}

/**
* Estimates initial position using ASIR.
*
* TPose initPos is the estimate of the initial position
* int size, is the size of the particle distribution used for estimation
* float v[3] is the odometry estimate for ASIR step
* Q[3] is the uncertainty of the initial distribution	  
* TLineVector cMap is the line map
* float *scan is the actual measurement taken from initPose + v[3]
*
* Returns estimated pose distribution
*/ 
TPoseParticleDistribution estimateInitialPosition( 
												  TPose initPose,
												  int size,
												  float v[3],
									   			  float Q[3],
									   			  TLineVector cMap,
												  float *scan)
{
	TPoseParticleDistribution PPD;
	float Qp[3]; // "Prediction error
	//	float Qp0[3]={0,0,0};



	Qp[0] = (float)0.2; // x-variance
	Qp[1] = (float)0.2; // y-variance
	Qp[2] = (float)(2.0 * PI / 180.0); // angle STD!!!

	PPD = getInitialDistribution(initPose,Q,size);
	//PPD = ASIRStep(PPD,v,Qp0,cMap,scan);
	//PPD = ASIRStep(PPD,v,Qp,cMap,scan);

	return PPD;
}

TPoseParticleDistribution MCLBasicStep(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan)
{
	PPD = penaPredict(PPD,v,Q);
	//mexPrintf("Likelihood...\n");

	PPD = calculateLikelihood(PPD,cMap,scan);
	//mexPrintf("Normalize...\n");

	PPD = normalizeDistribution(PPD);
	//mexPrintf("SIR...\n");

	PPD = SIRupdate(PPD);

	v[0] = 0.0f;
	v[1] = 0.0f;
	v[2] = 0.0f;

	PPD = penaPredict(PPD,v,Q);



return PPD;
}

TPoseParticleDistribution MCLBasicStepNoSIR(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan)
{
	PPD = penaPredict(PPD,v,Q);
	//mexPrintf("Likelihood...\n");

	PPD = calculateLikelihood(PPD,cMap,scan);
	//mexPrintf("Normalize...\n");

	PPD = normalizeDistribution(PPD);
	//mexPrintf("SIR...\n");

return PPD;
}

TPoseParticleDistribution MCLBasicCovStep(
										  TPoseParticleDistribution PPD,
										  float v[3],
										  float Q[4],
										  TLineVector cMap,
										  float *scan)
{
	PPD = penaPredictCov(PPD,v,Q);
	//mexPrintf("Likelihood...\n");

	PPD = calculateLikelihood(PPD,cMap,scan);
	//mexPrintf("Normalize...\n");

	PPD = normalizeDistribution(PPD);
	//mexPrintf("SIR...\n");

	PPD = SIRupdate(PPD);

return PPD;
}

TPoseParticleDistribution ASIRStep(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan)
{	
	TPoseParticleDistribution tmp;
	int i;

	tmp = newPoseParticleDistribution(PPD.NumOfParticles);
	
	for(i=0;i<PPD.NumOfParticles;i++){
		tmp.Particles[i] = PPD.Particles[i];
		tmp.NumOfParticles++;
		
	}
	
	//mexPrintf("nPPD: %d, nTmp:%d\n",PPD.NumOfParticles,tmp.NumOfParticles);
	//for(i=0;i<3;i++)
	//	mexPrintf("v[%d]:%f Q[%d]:%f\n",i,v[i],i,Q[i]);

	//mexPrintf("Predict...\n");
	tmp = penaPredict(tmp,v,Q);
	//mexPrintf("Likelihood...\n");

	tmp = calculateLikelihood(tmp,cMap,scan);
	//mexPrintf("Normalize...\n");

	tmp = normalizeDistribution(tmp);
	
	
	for(i=0;i<PPD.NumOfParticles;i++){
		PPD.Particles[i].p = tmp.Particles[i].p;
	}
	//mexPrintf("SIR...\n");
	PPD = SIRupdate(PPD);
	
	PPD = penaPredict(PPD,v,Q);
	//mexPrintf("Likelihood...\n");

	PPD = calculateLikelihood(PPD,cMap,scan);
	//mexPrintf("Normalize...\n");

	PPD = normalizeASIRDistribution(PPD,tmp);
	//mexPrintf("SIR...\n");

	PPD = SIRupdate(PPD);

	freePoseParticleDistribution(tmp);
return PPD;
}
